/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 膨胀层文件
 *@author tuyinan
 *@data 2022-04-xx
 */
#ifndef COSTMAP_2D_INFLATION_LAYER_H_
#define COSTMAP_2D_INFLATION_LAYER_H_

#include <costmap_2d/layer.h>
#include <costmap_2d/layered_costmap.h>

#include <boost/thread.hpp>

namespace costmap_2d {
/**
 *@brief 存储核信息
 *@author tuyinan
 *@version 2022-04-xx
 */
class CellData {
 public:
  /**
   *@brief 构造函数
   *@param double i 该核在代价地图的编号
   *@param unsigned int x 核的横坐标
   *@param unsigned int y 核的纵坐标
   *@param unsigned int sx 该核最近的障碍物的核的横坐标
   *@param unsigned int sy 该核最近的障碍物的核的纵坐标
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  CellData(double i, unsigned int x, unsigned int y, unsigned int sx,
           unsigned int sy)
      : index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
  /// 编号
  unsigned int index_;
  /// 横坐标，纵坐标
  unsigned int x_, y_;
  /// 离该核最近的障碍物核的横坐标，纵坐标
  unsigned int src_x_, src_y_;
};

/**
 *@brief 膨胀层
 *@author tuyinan
 *@version 2022-04-xx
 */
class InflationLayer : public Layer {
 public:
  /**
   *@brief 构造函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  InflationLayer();
  /**
   *@brief 析构函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual ~InflationLayer() {
    deleteKernels();
    if (seen_) {delete[] seen_;}
  }
  /**
   *@brief 初始化函数
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void onInitialize();
  /**
   *@brief 更新地图边际
   *@param double robot_x 机器人位姿x
   *@param double robot_y 机器人位姿y
   *@param double robot_yaw 机器人角度
   *@param double* min_x 最小x
   *@param double* min_y 最小y
   *@param double* max_x 最大x
   *@param double* max_y 最大y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
                            double* min_x, double* min_y, double* max_x,
                            double* max_y);
  /**
   *@brief 更新代价
   *@param costmap_2d::Costmap2D& 代价地图类
   *@param int min_i 最小的x
   *@param int min_j 最小的y
   *@param int max_i 最大的x
   *@param int max_j 最大的y
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
                           int min_j, int max_i, int max_j);
  /**
   *@brief 地图大小适配主代价地图
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void matchSize();
  /**
   *@brief 重置
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void reset() { onInitialize(); }
  /**
   *@brief 根据距离计算代价
   *@param double distance 距离
   *@return unsigned char 值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual inline unsigned char computeCost(double distance) const {
    unsigned char cost = 0;
    if (distance == 0) {
      cost = LETHAL_OBSTACLE;
    } else if (distance * resolution_ <= inscribed_radius_) {
      cost = INSCRIBED_INFLATED_OBSTACLE;
    } else {
      // make sure cost falls off by Euclidean distance
      double euclidean_distance = distance * resolution_;
      double factor =
          exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
      cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
    }
    return cost;
  }
  /**
   *@brief 设置膨胀参数
   *@param double inflation_radius 膨胀半径
   *@param double cost_scaling_factor 代价缩减系数
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void setInflationParameters(double inflation_radius,
                              double cost_scaling_factor);

 protected:
  /**
   *@brief 修改占位来适配主程序的占位
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  virtual void onFootprintChanged();
  /// 一把小锁
  boost::recursive_mutex* inflation_access_;

  /// 转换值
  double resolution_;
  /// 膨胀半径
  double inflation_radius_;
  /// 机器人最大半径
  double inscribed_radius_;
  /// 代价缩减系数
  double weight_;
  /// 是否膨胀未知区域
  bool inflate_unknown_;

 private:
  /**
   *@brief 从距离存储空间查找距离
   *@param int mx 该核的横坐标
   *@param int my 该核的纵坐标
   *@param int src_x 该核的src核的横坐标
   *@param int src_y 该核的src核的纵坐标
   *@return double 值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  inline double distanceLookup(int mx, int my, int src_x, int src_y) {
    unsigned int dx = abs(mx - src_x);
    unsigned int dy = abs(my - src_y);
    return cached_distances_[dx][dy];
  }
  /**
   *@brief 从代价存储空间查找代价
   *@param int mx 该核的横坐标
   *@param int my 该核的纵坐标
   *@param int src_x 该核的src核的横坐标
   *@param int src_y 该核的src核的纵坐标
   *@return double 值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  inline unsigned char costLookup(int mx, int my, int src_x, int src_y) {
    unsigned int dx = abs(mx - src_x);
    unsigned int dy = abs(my - src_y);
    return cached_costs_[dx][dy];
  }
  /**
   *@brief 预计算代价和距离存储空间
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void computeCaches();
  /**
   *@brief 删除核
   *@param 无
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void deleteKernels();
  /**
   *@brief 膨胀一块区域
   *@param int min_i 最小的x
   *@param int min_j 最小的y
   *@param int max_i 最大的x
   *@param int max_j 最大的y
   *@param unsigned char* master_grid 地图
   *@return void
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  void inflateArea(int min_i, int min_j, int max_i, int max_j,
                   unsigned char* master_grid);
  /**
   *@brief 距离计算核距离
   *@param double world_dist 实际距离
   *@return int 核距离
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  unsigned int cellDistance(double world_dist) {
    return layered_costmap_->getCostmap()->cellDistance(world_dist);
  }
  /**
   *@brief 添加核信息
   *@param unsigned int index 核编号
   *@param int mx 该核的横坐标
   *@param int my 该核的纵坐标
   *@param int src_x 该核的src核的横坐标
   *@param int src_y 该核的src核的纵坐标
   *@return double 值
   *@author: tuyinan
   *@version: 2022-04-xx
   */
  inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
                      unsigned int src_x, unsigned int src_y);
  /// 核膨胀半径
  unsigned int cell_inflation_radius_;
  /// 预计算得到的核膨胀半径
  unsigned int cached_cell_inflation_radius_;
  /// 膨胀核集
  std::map<double, std::vector<CellData> > inflation_cells_;
  /// 是否可视
  bool* seen_;
  /// 可视大小
  int seen_size_;
  /// 预计算的代价集
  unsigned char** cached_costs_;
  /// 预计算的距离集
  double** cached_distances_;
  /// 记录的最小x，最小y，最大x，最大y
  double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
  /// 是否需要重新计算全部膨胀
  bool need_reinflation_;
};

}  // namespace costmap_2d

#endif  // COSTMAP_2D_INFLATION_LAYER_H_
